﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using WebDriver;

namespace WaterRobotMonitor
{
    public partial class Form1 : Form
    {
        public WaterRobot Robot = new WaterRobot();
        public MapServer MapServer = new MapServer();
        public FormZAxis FormZ = new FormZAxis();

#region "delegates"
        public delegate void DeleVoid();
        public delegate void DeleDoubleDouble(double x,double y);
        public DeleVoid deleRefreshMarker;
        public DeleDoubleDouble deleRefreshPosition;
        #endregion

        public int RobotJogTick = 0;
        public const int RobotJogReleaseThreshold = 3;

        private List<ZedGraph.PointPair> map_points = new List<ZedGraph.PointPair>
        {
            new ZedGraph.PointPair( 6, -5 ),
            new ZedGraph.PointPair( 6, -10 ),
            new ZedGraph.PointPair( 0, -10 ),
            new ZedGraph.PointPair( 0, -5 ),
            new ZedGraph.PointPair( 6, -5 )
        };
        public Form1()
        {
            InitializeComponent();
            deleRefreshMarker = RefreshMarkerList;
            deleRefreshPosition = RefreshRobotPosition;
            Robot.Driver.OnGetMarkerList += (markers) =>
            {
                Invoke(deleRefreshMarker);
            };
            Robot.Driver.OnGetRobotStatus += () =>
            {
                Invoke(
                    deleRefreshPosition, 
                    Robot.Driver.Status.current_pose.x, 
                    Robot.Driver.Status.current_pose.y
                    );
            };
        }

        private void Form1_Load(object sender, EventArgs e)
        {
            var pane = zedMap.GraphPane;
            pane.Title.Text = "";
            pane.XAxis.Title.Text = "";
            pane.YAxis.Title.Text = "";
            pane.YAxis.Scale.Min = -11;
            pane.YAxis.Scale.Max = -4;
            pane.XAxis.Scale.Min = -1;
            pane.XAxis.Scale.Max = 7;
            pane.XAxis.MajorGrid.IsVisible = true;
            pane.YAxis.MajorGrid.IsVisible = true;
            pane.IsFontsScaled = false; //禁用自动调整字体大小
            pane.Title.FontSpec.Size = 12.0F; //设置标题字体
            RefreshRobotPosition(0, 0);
            MapServer.CreateMap(map_points);
            timer1.Start();
            //TxtIP.Text = "127.0.0.1";
        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            Robot.Driver.GetRobotStatus();

            if (!checkManul.Checked)
                return;

            /* 手动控制指令 */
            if(Robot.JogMoving)
            {
                var vx = WaterRobot.MaxVx / 100.0 * VxBar.Value * Robot.VxDir;
                var vth = WaterRobot.MaxVth / 100.0 * VthBar.Value * Robot.VthDir;
                Robot.SetVel(vx, vth);
                RobotJogTick = 0;
            }
            else
            {
                if (RobotJogTick++ == RobotJogReleaseThreshold)
                {
                    RobotJogTick = 0;
                    Robot.JogMoving = false;
                    Robot.Vx = 0;
                    Robot.Vth = 0;
                    Robot.Driver.SetVelocity(0, 0);
                }
            }
        }

        public void RefreshMarkerList()
        {
            if (!(Robot.Driver.MarkerLists is null))
            {
                boxMarkers.Items.Clear();
                for (int i = 0; i < Robot.Driver.MarkerLists.Length; i++)
                    boxMarkers.Items.Add(Robot.Driver.MarkerLists[i]);
            }
            boxMarkers.Text = boxMarkers.Items[0].ToString();
        }

        public void RefreshRobotPosition(double x, double y)
        {
            var pane = zedMap.GraphPane;
            Robot.SetPos(x, y);
            Robot.Battery = Robot.Driver.Status.power_percent;
            lbRobotX.Text = $"X: {Robot.Position.X :F3}";
            lbRobotY.Text = $"Y: {Robot.Position.Y :F3}";
            lbBattery.Text = $"Battery: {Robot.Battery}%";
            pane.CurveList.Clear();
            var pos = pane.AddCurve("", Robot.XList, Robot.YList, Color.Red);
            var map = pane.AddCurve("", MapServer.XList.ToArray(), MapServer.YList.ToArray(), Color.Green);
            map.Line.Width = 5;
            map.Symbol.Type = ZedGraph.SymbolType.None;
            pos.Symbol.Type = ZedGraph.SymbolType.Circle;
            pos.Symbol.Fill.Color = Color.Black;
            pos.Symbol.Border.Width = 2;
            pos.Symbol.Size = 10;
            pane.AxisChange();
            zedMap.Refresh();
        }

        private void Form1_Resize(object sender, EventArgs e)
        {
            var size = zedMap.Size;
            size.Width = size.Height;
            zedMap.Size = size;
        }

        private void zedMap_MouseClick(object sender, MouseEventArgs e)
        {
            double x = e.Location.X;
            double y = e.Location.Y;
            zedMap.GraphPane.ReverseTransform(e.Location, out x, out y);
            Console.WriteLine($"({x},{y})");
        }

        private void btnConnect_Click(object sender, EventArgs e)
        {
            if (btnConnect.Text == "Connect")
                Robot.Driver.TcpConnect(TxtIP.Text, Robot.Driver.CmdPort);
            else
                Robot.Driver.Close();

            if (Robot.Driver.Ready)
            {
                btnConnect.Text = "Close";
                if (!Robot.Driver.IsReceiving) Robot.Driver.StartReceiving();
            }
            else
            {
                btnConnect.Text = "Connect";
            }
        }

        private void btnGetRobotInfo_Click(object sender, EventArgs e)
        {
            Robot.Driver.GetRobotInfo();
        }

        private void btnGetRobotStatus_Click(object sender, EventArgs e)
        {
            Robot.Driver.GetRobotStatus();
        }

        private void btnGetMarkerInfo_Click(object sender, EventArgs e)
        {
            Robot.Driver.GetMarkersInfo();
        }

        private void btnGotoMarker_Click(object sender, EventArgs e)
        {
            if(boxMarkers.Text.Length == 0)
            {
                MessageBox.Show("No Available Marker!");
                return;
            }
            Robot.Driver.MoveToMarker(boxMarkers.Text);
        }

        private void btnConnectCamera_Click(object sender, EventArgs e)
        {

        }

        private void btnMoveF_MouseDown(object sender, MouseEventArgs e)
        {
            Robot.JogMoving = true;
            Robot.VxDir = 1;
        }

        private void btnMoveB_MouseDown(object sender, MouseEventArgs e)
        {
            Robot.JogMoving = true;
            Robot.VxDir = -1;
        }

        private void btnMove_MouseUp(object sender, MouseEventArgs e)
        {
            Robot.JogMoving = false;
        }

        private void btnTurnLeft_MouseDown(object sender, MouseEventArgs e)
        {
            Robot.JogMoving = true;
            Robot.VthDir = 1;
        }

        private void btnTurnRight_MouseDown(object sender, MouseEventArgs e)
        {
            Robot.JogMoving = true;
            Robot.VthDir = -1;
        }

        private void btnZAxis_Click(object sender, EventArgs e)
        {
            FormZ.Show();
        }
    }

    public class MapServer
    {
        public List<double> XList = new List<double>();
        public List<double> YList = new List<double>();

        public void CreateMap(List<ZedGraph.PointPair> points)
        {
            XList.Clear();
            YList.Clear();
            for(int i=0;i<points.Count;i++)
            {
                XList.Add(points[i].X);
                YList.Add(points[i].Y);
            }
        }
    }


    public class WaterRobot
    {
        public ZedGraph.PointPair Position = new ZedGraph.PointPair();
        public RobotPort Driver = new RobotPort();
        public int Battery { get; set; } = 0;
        public double Vx { get; set; } = 0.1;
        public double Vth { get; set; } = 0.0;
        public bool JogMoving { get; set; } = false; //手动移动命令启动
        public int VxDir { get; set; } = 1; // 直行方向
        public int VthDir { get; set; } = 1; // 旋转方向
        public const double MaxVx = 0.5;
        public const double MaxVth = 1.0;

        private double[] _XList = new double[1];
        private double[] _YList = new double[1];
        public double[] XList
        {
            get 
            {
                _XList[0] = Position.X;
                return _XList; 
            }
        }

        public double[] YList
        {
            get
            {
                _YList[0] = Position.Y;
                return _YList;
            }
        }

        public void SetPos(double x, double y)
        {
            Position.X = x;
            Position.Y = y;
        }

        public void SetVel(double vx, double vth)
        {
            Vx = vx;
            Vth = vth;
            Driver.SetVelocity(Vx, Vth);
        }

    }
}
